#ifndef MASSSPRINGDAMPER_H
#define MASSSPRINGDAMPER_H

////////////////////////////////////////////////////////////////////////////////

#include <math.h>

////////////////////////////////////////////////////////////////////////////////

/** Mass-Spring-Damper model class. */
class MassSpringDamper
{
public:

    /** State vector indecies. */
    enum StateIndecies
    {
        X = 0,  ///< position
        V = 1   ///< velocity
    };

    /** Constructor. */
    MassSpringDamper() :
        m_mass ( 1.0 ),

        m_stifCoef ( 1.0 ),
        m_dampCoef ( 0.5 )
    {
//        double omega_0 = sqrt( m_stifCoef / m_mass );
//        double zeta = m_dampCoef / ( 2 * sqrt( m_stifCoef * m_mass ) );
    }

    /**
	 * Computes current state derivatives vector.
     * @param 2-dimensional current state vector
     * @param resulting 2-dimensional current state derivatives vector */
    void computeDeriv( const double *s, double *ds_dt )
    {
        // F = - k*x - c*V
        double force = - m_stifCoef * s[ X ] - m_dampCoef * s[ V ];

        ds_dt[ X ] = s[ V ];            // x' = V
        ds_dt[ V ] = force / m_mass;    // V' = F / m
    }

    /**
	 * Computes state vector in a given time using analytical solution
     * of Mass-Spring-Damper motion equation (2nd-order homogeneous linear
     * differential equation)
     * @param 2-dimensional state vector initial value
     * @param resulting 2-dimensional state vector
     * @param time [s] */
    void computeState( const double *s_0, double *s, double t )
    {
        // F = - k*x - c*V
        // a = F / m
        // a = dV_dt ( a = V' )
        // dV_dt = -k/m * x - c/m * V
        // V = dx_dt ( V = x')
        // x'' = - k/m * x - c/m * x'
        // x'' + c/m * x' + k/m * x = 0
        //
        // assume x(t) = e^(r*t)
        // then x' = r * e^(r*t)  ,  x'' = r^2 * e^(r*t)
        //
        // r^2 * e^(r*t)  +  c/m * r * e^(r*t)  +  k/m * e^(r*t)  =  0
        // r^2 + c/m * r + k/m = 0
        //
        // A * r^2 + B * r + C = 0
        // where:
        // A = 1
        // B = c/m
        // C = k/m
        double a = 1.0;
        double b = m_dampCoef / m_mass;
        double c = m_stifCoef / m_mass;

        // delta = B^2 - 4*A*C
        double delta = b*b - 4.0*a*c;

        if ( fabs(delta) < 10.0e-9 ) delta = 0.0;

        // if delta > 0
        if ( delta > 0.0 )
        {
            double r_1 = ( -b - sqrt( delta ) ) / ( 2.0 * a );
            double r_2 = ( -b + sqrt( delta ) ) / ( 2.0 * a );

            // x (t) = C_1 * e^(r_1*t) + C_2 * e^(r_2*t)
            // x'(t) = C_1 * r_1 * e^(r_1*t) + C_2 * r_2 *e^(r_2*t)
            // x (0) = C_1 + C_2
            // x'(0) = C_1 * r_1 + C_2 * r_2
            // x (0) = x_0
            // x'(0) = V_0
            //
            // x_0 = C_1 + C_2
            // V_0 = C_1 * r_1 + C_2 * r_2
            //
            // C_1 = ( V_0 - x_0 * r_2 ) / ( r_1 - r_2 )
            // C_2 = x_0 - C_1
            double C_1 = ( s_0[ V ] - s_0[ X ] * r_2 ) / ( r_1 - r_2 );
            double C_2 = s_0[ X ] - C_1;

            // x (t) = C_1 * e^(r_1*t) + C_2 * e^(r_2*t)
            s[ X ] = C_1 * exp(r_1*t) + C_2 * exp(r_2*t);

            // x'(t) = C_1 * r_1 * e^(r_1*t) + C_2 * r_2 *e^(r_2*t)
            s[ V ] = C_1 * r_1 * exp(r_1*t) + C_2 * r_2 * exp(r_2*t);
        }
        else if ( delta == 0.0 )
        {
            double r = -b / ( 2.0 * a );

            // x (t) = ( C_1*t + C_2 ) * e^(r*t)
            // x'(t) = [ C_1 + r*( C_1*t + C_2 ) ] * e^(r*t)
            // x (0) = C_2
            // x'(0) = C_1 + r*C_2
            //
            // C_2 = x_0
            // C_1 = V_0 - r*C_2
            double C_2 = s_0[ X ];
            double C_1 = s_0[ V ] - r*C_2;

            // x (t) = ( C_1*t + C_2 ) * e^(r*t)
            s[ X ] = ( C_1*t + C_2 ) * exp(r*t);

            // x'(t) = [ C_1 + r*( C_1*t + C_2 ) ] * e^(r*t)
            s[ V ] = ( C_1 + r*( C_1*t + C_2 ) ) * exp(r*t);
        }
        else if ( delta < 0.0 )
        {
            double r_Re = -b / ( 2.0 * a );
            double r_Im = sqrt( 4.0*a*c - b*b ) / ( 2.0 * a );

            // x (t) = e^(r_Re*t) * ( C_1*cos(r_Im*t) + C_2*sin(r_Im*t) )
            // x'(t) = e^(r_Re*t) * [ (C_1*r_Re + C_2*r_Im)*cos(r_Im*t) + (C_2*r_Re - C_1*r_Im)*sin(r_Im*t) ]
            // x (0) = C_1
            // x'(0) = C_1*r_Re + C_2*r_Im
            //
            // C_1 = x_0
            // C_2 = ( V_0 - C_1*r_Re ) / r_Im
            double C_1 = s_0[ X ];
            double C_2 = ( s_0[ V ] - C_1*r_Re ) / r_Im;

            // x (t) = e^(r_Re*t) * ( C_1*cos(r_Im*t) + C_2*sin(r_Im*t) )
            s[ X ] = exp(r_Re*t) * ( C_1*cos(r_Im*t) + C_2*sin(r_Im*t) );

            // x'(t) = e^(r_Re*t) * [ (C_1*r_Re + C_2*r_Im)*cos(r_Im*t) + (C_2*r_Re - C_1*r_Im)*sin(r_Im*t) ]
            s[ V ] = exp(r_Re*t) * ( (C_1*r_Re + C_2*r_Im)*cos(r_Im*t) + (C_2*r_Re - C_1*r_Im)*sin(r_Im*t) );
        }
    }

private:

    const double m_mass;        ///< [kg] mass
    const double m_stifCoef;    ///< [N/m] spring stiffness coefficient
    const double m_dampCoef;    ///< [N*s/m] damping coefficient
};

////////////////////////////////////////////////////////////////////////////////

#endif // MASSSPRINGDAMPER_H
